#include "Robot.hpp"
#include "Sphere.hpp"
#include "Room.hpp"

#include "Debug.hpp"

#include "glm/glm.hpp"
#include "glm/gtc/matrix_transform.hpp"

using glm::mat4;
using glm::vec3;

extern Sphere * g_sphere;
extern Room * g_room;

Robot::Robot(glm::mat4 model, Object * parent)
  : Object(model, parent)
{
   m_bodyRadius = 0.5f;
   m_bodyHeight = 2.0f;
   m_armHeight = 2.0f;
   m_headHeight = 0.5f;
   m_wheelDiam = 0.5f;
   m_smallDim = 0.25f;
  
   m_position = vec3(0.0f, 0.0f, 10.0f);
   m_bodyAngle = 0.0f;
   m_armAngle = 0.0f;
   m_state = Entering;
   
   vec3 grey(0.5f, 0.5f, 0.5f);
   
   mat4 matrix = glm::translate(mat4(1.0f),
                          vec3(0.0f, m_wheelDiam + m_bodyHeight/2, 0.0f));
   
   m_body = new Cylinder(matrix,
                         m_bodyRadius,
                         m_bodyHeight,
                         32,
                         this);
   
   matrix = glm::translate(mat4(1.0f),
                          vec3(0.0f, m_wheelDiam/2, -m_bodyRadius/2));
   mat4 RotMat = glm::rotate(mat4(1.0f), 90.0f,
                             vec3(0.0, 0.0, 1.0f));
   m_frontWheel = new Cylinder(matrix * RotMat,
                         m_wheelDiam/2,
                         m_smallDim,
                         32,
                         this);
   
   matrix = glm::translate(mat4(1.0f),
                          vec3(-m_bodyRadius/2, m_wheelDiam/2, m_bodyRadius/2));
   m_leftWheel = new Cylinder(matrix * RotMat,
                              m_wheelDiam/2,
                              m_smallDim,
                              32,
                              this);
   
   matrix = glm::translate(mat4(1.0f),
                          vec3(+m_bodyRadius/2, m_wheelDiam/2, m_bodyRadius/2));
   m_rightWheel = new Cylinder(matrix * RotMat,
                               m_wheelDiam/2,
                               m_smallDim,
                               32,
                               this);
 
   
   
   
   matrix = leftArmModel();
   m_leftArm = new Cube(matrix, m_smallDim, m_armHeight, m_smallDim, grey, this);
   
   matrix = rightArmModel();
   m_rightArm = new Cube(matrix, m_smallDim, m_armHeight, m_smallDim, grey, this);
   
   matrix = glm::translate(mat4(1.0f),
                          vec3(0.0f, m_wheelDiam + m_bodyHeight + m_headHeight/2,
                               0.0f));
   
   m_head = new Cube(matrix, m_smallDim, m_headHeight, m_smallDim, grey, this);
  
}

mat4 Robot::leftArmModel()
{
  
  mat4 mat1 = glm::translate(mat4(1.0f),
                             vec3(0.0f,
                                  -(m_armHeight - m_smallDim)/2,
                                  0.0f));
  mat4 rot = glm::rotate(mat4(1.0f), m_armAngle,
                         vec3(1.0f, 0.0f, 0.0f));
  
  mat4 position = glm::translate(mat4(1.0f),
                                 vec3(-(m_bodyRadius + m_smallDim/2),
                                      m_wheelDiam + m_bodyHeight, 0.0f));
  
  return position*rot*mat1;
}

mat4 Robot::rightArmModel()
{
  
  mat4 mat1 = glm::translate(mat4(1.0f),
                             vec3(0.0f,
                                  -(m_armHeight - m_smallDim)/2,
                                  0.0f));
  mat4 rot = glm::rotate(mat4(1.0f), m_armAngle,
                         vec3(1.0f, 0.0f, 0.0f));
  
  mat4 position = glm::translate(mat4(1.0f),
                                 vec3((m_bodyRadius + m_smallDim/2),
                                      m_wheelDiam + m_bodyHeight, 0.0f));
  
  return position * rot * mat1;
}



void Robot::updateRobot()
{
  const float movementStepSize = 0.1f;
  const float bodyRotationStepSize = 1.0f;
  
  const float triangleLength = 4.0f;
  
  mat4 rotMat = glm::rotate(mat4(1.0f), m_bodyAngle,
                                vec3(0.0f, 1.0f, 0.0f));
  
  mat4 model;
  
  /* path taken by robot. Triangle length L, 
   *    +-------+
   *     \     /
   *      \   /L
   *       \ / 
   *        +  
   *        |  
   *        |
   *        |
   */        
  
  switch (m_state) {
    case Entering:
      m_position += vec3(0.0f, 0.0f, -movementStepSize);
      if (m_position.z <= 0.0f) {
        m_position.z = 0.0f;
        m_state = RotatingToTable1; 
      }
      break;
      
    case RotatingToTable1:
      m_bodyAngle -= bodyRotationStepSize;
      m_armAngle += 90.0f / 30.0f * bodyRotationStepSize;
      if (m_bodyAngle <= - 30.0f) {
        m_bodyAngle = -30.0f;
        m_armAngle = 90.0f;
        m_state = MovingToTable1;
        m_param = 0.0f; // used by next state
      }
      break;
      
    case MovingToTable1:
      m_param += movementStepSize;
      if (m_param >= triangleLength) {
        m_position = glm::mat3(rotMat) * vec3(0.0f, 0.0f, -triangleLength);
        m_state = RotatingToTable2;
        m_param = 0.0f; // used by next state
        
        // pickup sphere
        Object::attachChild(g_sphere);
        model = glm::translate(mat4(1.0f), vec3(0.0f, 2.5f, -1.5f));
        g_sphere->setModel(model);
        
      } else {
          m_position = glm::mat3(rotMat) * vec3(0.0f, 0.0f, -m_param);
      }
      break;
      
    case RotatingToTable2:
      m_bodyAngle += bodyRotationStepSize;
      if (m_bodyAngle >= 90.0f) {
        m_bodyAngle = 90.0f;
        m_state = MovingToTable2;
      }
      break;
    
    case MovingToTable2:
      m_param += movementStepSize;
      m_position.x -= movementStepSize;
      if (m_param >= triangleLength) {
        m_position.x = -triangleLength/2;
        m_state = RotatingAwayfromOrigin;
        
      }
      break;
      
    case RotatingAwayfromOrigin:
      m_bodyAngle -= bodyRotationStepSize;
      if (m_bodyAngle <= 30.0f) {
        m_bodyAngle = 30.0f;
        m_state = ReversingToOrigin;
        m_param = triangleLength; // used by next state
        
        // drop sphere
        g_room->attachChild(g_sphere);
        model = glm::rotate(mat4(1.0f), 30.0f, vec3(0.0f, 1.0f, 0.0f)) *
                glm::translate(mat4(1.0f), vec3(0.0, 2.5f, -5.5f));
       g_sphere->setModel(model);
      }
      break;
      
    case ReversingToOrigin:
      m_param -= movementStepSize;
      if (m_param <= 0.0f) {
        m_position = vec3(0.0f, 0.0f, 0.0f);
        m_state = RotatingToDoor;
      } else {
          m_position = glm::mat3(rotMat) * vec3(0.0f, 0.0f, -m_param);
      }
      break;
      
    case RotatingToDoor:
      m_bodyAngle += bodyRotationStepSize;
      m_armAngle -= 90.0f / 150.0f * bodyRotationStepSize;

      if (m_bodyAngle >= 180.0f) {
        m_bodyAngle = 180.0f;
        m_armAngle = 0.0f;
        m_state = Leaving;
      }
      break;
      
    case Leaving:
      m_position += vec3(0.0f, 0.0f, movementStepSize);
      if (m_position.z >= 10.0f) {
        m_position.z = 10.0f;
        m_state = Stop; 
      }
      break;
      
    case Stop:
      break;
  }
  
  model = glm::translate(mat4(1.0f), m_position) * 
          glm::rotate(mat4(1.0f), m_bodyAngle,
                      vec3(0.0f, 1.0f, 0.0f));

  m_leftArm->setModel(leftArmModel());
  m_rightArm->setModel(rightArmModel());
               
  setModel(model);
  
}

glm::mat4 Robot::getCameraModel()
{
  mat4 rotation  = glm::rotate(mat4(1.0f), -m_bodyAngle,
                               vec3(0.0f, 1.0f, 0.0f));
  
  mat4 translation = glm::translate(mat4(1.0f),
      -vec3(0.0f, m_bodyHeight + m_wheelDiam + m_headHeight, 0.0f)  
      -m_position);
                 
  return rotation * translation;
}

/*
void Robot::draw(const glm::mat4 & modelview)
{
  body->draw(modelview);
  leftWheelModel->draw(modelview);
  rightWheelModel->draw(modelview);
  frontWheelModel->draw(modelview);
  
  head->draw(modelview);
}

robotLift*/